isometry_conversions.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
 
#pragma once
 
#include "core/common/wolf.h"
#include "core/composite/vector_composite.h"
#include "core/math/rotations.h"
 
namespace wolf
{
template <typename D1>
inline Eigen::Transform<typename D1::Scalar, 2, Eigen::Isometry> toIsometry2d(const Eigen::MatrixBase<D1>& translation,
                                                                              const double&                rotation)
{
    MatrixSizeCheck<2, 1>::check(translation);
 
    return Eigen::Translation<typename D1::Scalar, 2>(translation) * Eigen::Rotation2D<typename D1::Scalar>(rotation);
}
 
template <typename D1, typename D2>
inline Eigen::Transform<typename D1::Scalar, 2, Eigen::Isometry> toIsometry2d(const Eigen::MatrixBase<D1>& translation,
                                                                              const Eigen::MatrixBase<D2>& rotation)
{
    MatrixSizeCheck<2, 1>::check(translation);
    MatrixSizeCheck<1, 1>::check(rotation);
 
    return toIsometry2d(translation, rotation(0));
}
 
template <typename D1>
inline Eigen::Transform<typename D1::Scalar, 2, Eigen::Isometry> toIsometry2d(const Eigen::MatrixBase<D1>& trf)
{
    MatrixSizeCheck<3, 1>::check(trf);
 
    return toIsometry2d(Eigen::Matrix<typename D1::Scalar, 2, 1>(trf(0), trf(1)), trf(2));
}
 
inline Eigen::Isometry2d toIsometry2d(const VectorComposite& trf)
{
    return toIsometry2d(trf.at('P'), trf.at('O'));
}
 
template <typename D1, typename D2>
inline Eigen::Transform<typename D1::Scalar, 3, Eigen::Isometry> toIsometry3d(
    const Eigen::MatrixBase<D1>&     translation,
    const Eigen::QuaternionBase<D2>& rotation)
{
    MatrixSizeCheck<3, 1>::check(translation);
 
    return Eigen::Translation<typename D1::Scalar, 3>(translation) * rotation;
}
 
template <typename D1, typename D2>
inline Eigen::Transform<typename D1::Scalar, 3, Eigen::Isometry> toIsometry3d(const Eigen::MatrixBase<D1>& translation,
                                                                              const Eigen::MatrixBase<D2>& rotation)
{
    MatrixSizeCheck<3, 1>::check(translation);
    MatrixSizeCheck<4, 1>::check(rotation);
 
    return toIsometry3d(
        translation,
        Eigen::Quaternion<typename D2::Scalar>(Eigen::Matrix<typename D2::Scalar, 4, 1>(rotation).normalized()));
}
 
template <typename D1>
inline Eigen::Transform<typename D1::Scalar, 3, Eigen::Isometry> toIsometry3d(const Eigen::MatrixBase<D1>& trf)
{
    MatrixSizeCheck<7, 1>::check(trf);
    return toIsometry3d(Eigen::Matrix<typename D1::Scalar, 3, 1>(trf(0), trf(1), trf(2)),
                        Eigen::Quaternion<typename D1::Scalar>(trf(6), trf(3), trf(4), trf(5)));
}
 
inline Eigen::Isometry3d toIsometry3d(const VectorComposite& trf)
{
    return toIsometry3d(trf.at('P'), trf.at('O'));
}
 
inline Eigen::Vector3d toVector(const Eigen::Isometry2d& trf)
{
    return (Eigen::Vector3d() << trf.translation(), Eigen::Rotation2Dd(trf.rotation()).angle()).finished();
}
 
inline Eigen::Vector7d toVector(const Eigen::Isometry3d& trf)
{
    return (Eigen::Vector7d() << trf.translation(), Eigen::Quaterniond(trf.rotation()).coeffs().normalized())
        .finished();
}
 
inline Eigen::Isometry3d inverseProjectTo3d(const Eigen::Isometry2d& trf_2d)
{
    return Eigen::Translation3d((Eigen::Vector3d() << trf_2d.translation(), 0).finished()) *
           Eigen::AngleAxisd(Eigen::Rotation2Dd(trf_2d.rotation()).angle(), Eigen::Vector3d::UnitZ());
}
 
inline Eigen::Isometry2d projectTo2d(const Eigen::Isometry3d& trf_3d)
{
    return Eigen::Translation2d(trf_3d.translation().head<2>()) *
           Eigen::Rotation2Dd(getYaw(trf_3d.rotation().matrix()));
}
 
}  // namespace wolf